#include "main.h"

strMazeGrid stackMazeGrid[MAZE_ALL_X*MAZE_ALL_Y];       // 记录路径的堆栈数组
uint16_t stackTop = 0;                                  // 记录栈顶的变量

uint8_t mazeDebug[MAZE_ALL_Y][MAZE_ALL_X] = {           // 预设的迷宫墙壁信息
    {2,2,3,5,4,3,7,4},
    {9,15,12,2,3,12,10,2},
    {1,15,7,13,15,5,15,15},
    {3,12,10,1,14,2,10,10},
    {10,2,10,1,12,10,10,10},
    {10,9,13,7,5,13,13,14},
    {11,7,7,13,4,1,5,14},
    {8,8,9,5,5,5,5,12},
};

uint8_t mazeGet[MAZE_ALL_Y][MAZE_ALL_X] = {0};          // 获取到的迷宫信息
uint16_t mazeMap[MAZE_ALL_Y][MAZE_ALL_X] = {0};         // 用于获取最短路径的等高图
uint8_t bestPath[MAZE_ALL_X*MAZE_ALL_Y];                // 保存最佳路径转向步骤的数组

// 获得该坐标的墙面情况
void getGridWall(uint8_t x, uint8_t y, uint8_t* nowWell)
{
    *nowWell = mazeDebug[y][x];
}

// 由于传感器只能获得三面墙壁信息，车体后方的墙壁信息需要根据上一次行走方向推算
void fixGridWall(uint8_t direction, uint8_t* wall)
{
    switch(direction)
    {
        case 0x08:          // 上一步是向上走
            *wall |= 0x02;  // 将格子下方置为通路
        break;
        case 0x04:          // 上一步是向左走
            *wall |= 0x01;  // 将格子右方置为通路
        break;
        case 0x02:          // 上一步是向下走
            *wall |= 0x08;  // 将格子上方置为通路
        break;
        case 0x01:          // 上一步是向右走
            *wall |= 0x04;  // 将格子左方置为通路
        break;
    }
}

// 将一个格点作为走过的路径入栈，或者回溯时出栈
void pushPopOneGrid(uint8_t x, uint8_t y, uint8_t wall, uint8_t flagPP)
{
    if ( flagPP )   // flag为1表示入栈
    {
        stackTop++;
        stackMazeGrid[stackTop].thisX = x;
        stackMazeGrid[stackTop].thisY = y;
        stackMazeGrid[stackTop].GridWall = wall;
    }
    else            // flag为0表示出栈
    {
        stackTop--;
    } 
}

// 将走过的格子墙壁信息存在mazeGet[]数组里
void saveMazeBuff(uint8_t nowX, uint8_t nowY, uint8_t nowWall)
{
    mazeGet[nowY][nowX] = nowWall;
}

// 给出下一个要去格点的方向
uint8_t getNextGrid(uint8_t* direction, uint8_t* falgPP)
{
    uint8_t availableRout = 0;      // 记录可行方向的标志位

    // 统计可以走的方向
    if ( (stackMazeGrid[stackTop].GridWall & 0x08) &&       // 向上方向有路
         (stackMazeGrid[stackTop].thisY > 0) &&             // 向上方向不超过迷宫边界
         (mazeGet[stackMazeGrid[stackTop].thisY-1][stackMazeGrid[stackTop].thisX] == 0)     // 上边那格没有被走过
       )
    {
        availableRout |= 0x08;
    }

    if ( (stackMazeGrid[stackTop].GridWall & 0x04) &&       // 向左方向有路
         (stackMazeGrid[stackTop].thisX > 0) &&             // 向左方向不超过迷宫边界
         (mazeGet[stackMazeGrid[stackTop].thisY][stackMazeGrid[stackTop].thisX-1] == 0)     // 左边那格没有被走过
       )
    {
        availableRout |= 0x04;
    }

    if ( (stackMazeGrid[stackTop].GridWall & 0x02) &&       // 向下方向有路
         (stackMazeGrid[stackTop].thisY < MAZE_ALL_Y-1) &&  // 向下方向不超过迷宫边界
         (mazeGet[stackMazeGrid[stackTop].thisY+1][stackMazeGrid[stackTop].thisX] == 0)     // 下边那格没有被走过
       )
    {
        availableRout |= 0x02;
    }

    if ( (stackMazeGrid[stackTop].GridWall & 0x01) &&       // 向右方向有路
         (stackMazeGrid[stackTop].thisX < MAZE_ALL_X-1) &&  // 向右方向不超过迷宫边界
         (mazeGet[stackMazeGrid[stackTop].thisY][stackMazeGrid[stackTop].thisX+1] == 0)     // 右边那格没有被走过
       )
    {
        availableRout |= 0x01;
    }

    // 判断是否是走到死路的情况
	/*疑问：前面不是说车体走过的后方一定是通路吗，为什么会出现死路的情况呢？
	    答：在入栈阶段，在统计可以走的方向时，走过的格子就不再计入可以走的方向内了。
		    车体走过的后方属于走过的格子。
	*/
    if ( 0 == availableRout )
    {
        *falgPP = 0;                                                                        // 当前格需要出栈

        if ( stackMazeGrid[stackTop].thisX == stackMazeGrid[stackTop-1].thisX )             // 上一步是y方向走动
        {
            if ( stackMazeGrid[stackTop].thisY > stackMazeGrid[stackTop-1].thisY )          // 上一步是朝下走
                return(*direction = 0x08);
            else                                                                            // 上一步是朝上走 
                return(*direction = 0x02);
        }
        else
        {
            if ( stackMazeGrid[stackTop].thisX > stackMazeGrid[stackTop-1].thisX )          // 上一步是朝右走
                return(*direction = 0x04);
            else                                                                            // 上一步是朝左走
                return(*direction = 0x01);
        }
    }
    else
    {
        *falgPP = 1;
    }

    // 根据决策，找出优先级最高的方向
    if ( stackMazeGrid[stackTop].thisY < MAZE_ALL_X - stackMazeGrid[stackTop].thisX - 1 )   // 当前点在迷宫主对角线上方
    {
        if ( availableRout & 0x01 )                         // 按右上下左的优先级返回方向
            return(*direction = 0x01);
        if ( availableRout & 0x08 )
            return(*direction = 0x08);
        if ( availableRout & 0x02 )
            return(*direction = 0x02);
        if ( availableRout & 0x04 )
            return(*direction = 0x04);
    }
    else                                                                                    // 当前点在迷宫主对角线下方
    {
        if ( availableRout & 0x08 )                         // 按上右左下的优先级返回方向
            return(*direction = 0x08);
        if ( availableRout & 0x01 )
            return(*direction = 0x01);
        if ( availableRout & 0x04 )
            return(*direction = 0x04);
        if ( availableRout & 0x02 )
            return(*direction = 0x02);
    }
}

// 执行行走一步的函数
void moveOneStep(uint8_t direction, uint8_t* x, uint8_t* y)
{
    switch(direction)
    {
        case 0x08:
            *y -= 1;
        break;
        case 0x04:
            *x -= 1;
        break;
        case 0x02:
            *y += 1;
        break;
        case 0x01:
            *x += 1;
        break;
    }
}

// 根据探索阶段生成的mazeGet[],绘制等高图mazeMap[],最终求得最短路线保存在bestPath[]
void findBestPath(void)
{
    uint16_t price = 0;
    uint8_t testX = 0, testY = 0;

    // 初始化等高图
    for ( testX = 0; testX < MAZE_ALL_X; testX++ )                  
    {
        for ( testY = 0; testY < MAZE_ALL_Y; testY++ )
        {
            mazeMap[testY][testX] = 65535;
        }
    }
    mazeMap[MAZE_ALL_Y-1][0] = 1;                                   // 左下角等高值设为1

    // 绘制等高图
    for ( price = 1; mazeMap[0][MAZE_ALL_X-1] == 65535; price++ )   
    {
        for ( testX = 0; testX < MAZE_ALL_X; testX++ )              // 遍历整个迷宫等高图
        {
            for ( testY = 0; testY < MAZE_ALL_Y; testY++ )
            {
                if ( mazeMap[testY][testX] == price )               // 找到每个等高值为price的格子，将他们周围格子的等高值给出
                {
                    if ( (mazeGet[testY][testX] & 0x08) &&          // 向上方向有路
                         (testY > 0) &&                             // 向上方向不超过迷宫边界
                         (mazeMap[testY-1][testX] > price+1) )      // 上边那格等高值更大
                    {
                        mazeMap[testY-1][testX] = price+1;
                    }

                    if ( (mazeGet[testY][testX] & 0x04) &&          // 向左方向有路
                         (testX > 0) &&                             // 向左方向不超过迷宫边界
                         (mazeMap[testY][testX-1] > price+1) )      // 左边那格等高值更大
                    {
                        mazeMap[testY][testX-1] = price+1;
                    }

                    if ( (mazeGet[testY][testX] & 0x02) &&          // 向下方向有路
                         (testY < MAZE_ALL_Y-1) &&                  // 向下方向不超过迷宫边界
                         (mazeMap[testY+1][testX] > price+1) )      // 下边那格等高值更大
                    {
                        mazeMap[testY+1][testX] = price+1;
                    }

                    if ( (mazeGet[testY][testX] & 0x01) &&          // 向右方向有路
                         (testX < MAZE_ALL_X-1) &&                  // 向右方向不超过迷宫边界
                         (mazeMap[testY][testX+1] > price+1) )      // 右边那格等高值更大
                    {
                        mazeMap[testY][testX+1] = price+1;
                    }
                }
            }
        }
    }

    // 从终点反推最佳路径
    testY = 0;
    testX = MAZE_ALL_X-1;
    for ( price = mazeMap[0][MAZE_ALL_X-1]; price > 0; price-- )
    {
        if ( (mazeGet[testY][testX] & 0x08) &&          // 向上方向有路
             (testY > 0) &&                             // 向上方向不超过迷宫边界
             (mazeMap[testY-1][testX] == price-1) )     // 上边那格等高值合适
        {
            bestPath[price-2] = 0x02;
            testY--;
        }

        if ( (mazeGet[testY][testX] & 0x04) &&          // 向左方向有路
             (testX > 0) &&                             // 向左方向不超过迷宫边界
             (mazeMap[testY][testX-1] == price-1) )     // 左边那格等高值合适
        {
            bestPath[price-2] = 0x01;
            testX--;
        }

        if ( (mazeGet[testY][testX] & 0x02) &&          // 向下方向有路
             (testY < MAZE_ALL_Y-1) &&                  // 向下方向不超过迷宫边界
             (mazeMap[testY+1][testX] == price-1) )     // 下边那格等高值合适
        {
            bestPath[price-2] = 0x08;
            testY++;
        }

        if ( (mazeGet[testY][testX] & 0x01) &&          // 向右方向有路
             (testX < MAZE_ALL_X-1) &&                  // 向右方向不超过迷宫边界
             (mazeMap[testY][testX+1] == price-1) )     // 右边那格等高值合适
        {
            bestPath[price-2] = 0x04;
            testX++;
        }
    }
}

int main()
{
    // 迷宫探索算法
    uint8_t nowX = 0, nowY = MAZE_ALL_Y-1, nowWall = 0, direction = 0, flagPP = 1;
 //   pushPopOneGrid(0, MAZE_ALL_Y, 8, 1);      // 迷宫路径的起点在迷宫左下角往下一格的位置
    direction = 0x08;
    flagPP = 1;

    while(1)
    {
        printf("(%d,%d) ", nowX, nowY);

        getGridWall(nowX, nowY, &nowWall);      // 传感器获取当前格墙壁信息
        fixGridWall(direction, &nowWall);         // 根据上一次行走方向补全墙壁信息
        pushPopOneGrid(nowX, nowY, nowWall, flagPP);       // 将当前格入栈或出栈表示探索或回溯
        saveMazeBuff(nowX, nowY, nowWall);      // 将当前格迷宫信息存入记录数组
        getNextGrid(&direction, &flagPP);              // 给出下一步方向
        moveOneStep(direction, &nowX, &nowY);      //行走一步
        if ( nowX == MAZE_ALL_X-1 && nowY == 0 )
        {
            printf("(%d,%d) ", nowX, nowY);
            getGridWall(nowX, nowY, &nowWall);      // 传感器获取当前格墙壁信息
            fixGridWall(direction, &nowWall);         // 根据上一次行走方向补全墙壁信息
            saveMazeBuff(nowX, nowY, nowWall);      // 将当前格迷宫信息存入记录数组
            break;
        }
            
    }
    printf("  END!\n");

    findBestPath();
     uint16_t price = 0;
    for (price = 0; price < mazeMap[0][MAZE_ALL_X-1] - 1; price++ )   // 例如：经过10格走到终点，只需要走10-1=9步
        printf("%d, ", bestPath[price]);
    printf("END\n");

    //scanf("%d", &direction);
}


										/*迷宫算法注解-------2021.8.20-------wsj*/
/*

1.传感器获取当前格墙壁信息
2.根据上一次行走方向补全墙壁信息	  (此时才得到了一个格的完整墙壁信息)
3.将当前格入栈或出栈表示探索或回溯	(记录该格点的坐标以及墙壁信息到记录路径的堆栈数组stackMazeGrid[]里)
4.将该格点的墙壁信息存在mazeGet[]数组里
5.给出下一步的移动方向
6.移动一步updateXY();
重复循环以上6步，直至抵达迷宫终点
7.绘制等高图(以之前保存的墙壁信息mazeGetp[]为依据，进行3重for循环：从price=1开始循环，直至找到坐标为迷宫终点的点结束循环)
8.根据等高图解算出最优路径bestPath[]

至此，迷宫算法结束。
*/

